# -*- coding: utf-8 -*-
import threading
import queue
import time
import serial_init
from serial_init import SerialControl
import numpy as np
import argparse

class CustomControl:
    def __init__(self, args):
        self.mtx = threading.Lock()
        self.cv = threading.Condition(self.mtx)
        self.q = queue.Queue()
        self.stop_event = threading.Event()  # 用于通知线程退出
        self.publisher_thread = threading.Thread(target=self.pos_pub, daemon=True)
        self.subscriber_thread = threading.Thread(target=self.pos_sub, daemon=True)
        self.serial = SerialControl(args.port, baudrate=115200)
        self.target_pose = None
        self.local_pose = None

    def start(self):
        # 启动线程
        self.publisher_thread.start()
        self.subscriber_thread.start()

    def stop(self):
        # 通知线程退出
        self.stop_event.set()
        # 清空队列
        while not self.q.empty():
            self.q.get()
        # 关闭串口
        self.serial.close()

    def get_targetpose(self):
        self.target_pose = np.zeros(28)
        for i in range(90):
            if self.stop_event.is_set():  # 检查是否收到退出信号
                break
            self.target_pose[[1, 2, 3]] = 90-i
            time.sleep(0.1)
            print(self.target_pose)
        self.serial.close()

    def pos_pub(self):
        while not self.stop_event.is_set():  # 检查是否收到退出信号
            with self.cv:
                if self.q.empty() and self.target_pose is not None:
                    self.q.put(self.target_pose)
                    print("Thread-1 put_data:", self.target_pose)
                    self.cv.notify_all()
                else:
                    self.cv.wait(timeout=0.1)  # 添加超时以避免永久阻塞

    def pos_sub(self):
        while not self.stop_event.is_set():  # 检查是否收到退出信号
            with self.cv:
                if self.q.empty():
                    self.cv.wait(timeout=0.1)  # 添加超时以避免永久阻塞
                else:
                    data = self.q.get()
                    print(threading.current_thread().name + " get_data:", data)
                    cmd = self.serial.generate_write_command(data)
                    self.serial.send_command(cmd)
                    self.cv.notify()

    def pos_read(self):
        pass

def parse_opt():
    parser = argparse.ArgumentParser()
    parser.add_argument('--port', type=str, default='/dev/ttyUSB0', help='port name')
    opt = parser.parse_args()
    return opt

def main():
    opt = parse_opt()
    cc = CustomControl(opt)

    try:
        # 启动线程
        cc.start()

        # 在单独的线程中运行 get_targetpose
        targetpose_thread = threading.Thread(target=cc.get_targetpose, daemon=True)
        targetpose_thread.start()
        cc.serial.send_command("#021P1200T1000!")
        # 主线程等待
        while targetpose_thread.is_alive():
            time.sleep(0.1)

    except KeyboardInterrupt:
        print("\n收到 Ctrl+C 信号，正在退出...")
    finally:
        # 清理资源
        init_pose=np.zeros(28)
        cc.serial.send_command(cc.serial.generate_write_data(init_pose))
        cc.stop()
        print("程序已退出。")

if __name__ == "__main__":
    main()
